For control of a robot arm and the like, a servomotor for driving the arm includes position and speed control loops. The position of a driven body, such as the arm, is controlled by controlling the position of the servomotor. FIG. 2 is a block diagram of a servo control system for controlling one such driven body.
In FIG. 2, reference numeral 1 designates a position gain Kp in the position loop. Reference numerals 2 and 3 designate terms of a transfer function of the speed loop; 2, a term of an integrator; and K1, an integral gain. Reference numeral 3 designates a proportional term, and K2 a proportional gain. Reference numeral 4 designates a term of a transfer function of the motor; Kt, a torque constant; J, an inertia; value and A, a viscous friction term. Moreover, reference numeral 5 designates a transfer function by which a motor speed v is integrated to obtain a motor position y. Symbol s represents a Laplace operator.
A position deviation value .epsilon.p is obtained from a move command r and the motor position y. A speed command vc is obtained by multiplying the position deviation .epsilon.p by the position gain Kp. A speed deviation .epsilon.v is obtained from the speed command vc and the motor speed v. A torque command Tc is obtained by adding up the product of the integral gain K1 and a value obtained by integrating the speed deviation .epsilon.v by way of the integrator 2 and the product of the speed deviation .epsilon.v and the proportional gain K2. The motor 4 is driven according to this torque command Tc.
The control system for the driven body such as a robot arm, which is driven by ways of the servomotor and whose position is controlled, is generally controlled by way of a control system such as the one described above. Although PI control is used for the control of the speed loop in the aforesaid case of FIG. 2, IP control may be used instead, in some cases.
If the servomotor is controlled by the aforementioned control system, a driven body such as the robot arm moving toward a position assigned by a position command cannot keep away from an obstacle, if any, in the way, and continues to move toward a target position. If the accuracy of a workpiece is so low that there is some deviation between the command position and the position of the workpiece, while the robot is carrying out an operation to attach the workpiece to a machine tool by its hand, for example, the workpiece cannot be attached to the machine tool. In this case, moreover, it is difficult to move the robot arm manually, thereby moving the workpiece to a mounting position in the machine tool.
The reason is that the servomotor cannot move to the target position due to the presence of the obstacle though it is urged to move toward that position. Accordingly, the position deviation increases, so that the speed command Vc obtained by multiplying this position deviation by the position gain Kp also increases. Since the difference between the speed command Vc and the motor speed (this speed is "0" when engaged with the obstacle) is integrated by the integrator Z of the speed loop, the integral value in this integrator 2 gradually increases, so that the torque command Tc takes a large value. Thus, the servomotor outputs a maximum torque such that it reaches the target position. Accordingly, a human power .tau. cannot easily move the arm or the like to the intended position without being caught by the obstacle.